/**
 ****************************************************************************************************
 * @file        BSP_MPU6050.c
 * @author      Darren Tang
 * @email       247022864@qq.com
 * @version     V1.0
 * @date        2020-01-01
 * @brief       MPU6050应用
 ****************************************************************************************************
 * @attention
 *
 * 测试平台:STM32F103ZET6、STM32F407ZGT6、STM32H743IIT6
 *
 * 声明:此程序只用作学习，未经许可用作商业用途，必追究责任！
 *
 * 修改说明
 * V1.0 20200101发布
 * 第一次发布
 *
 ****************************************************************************************************
 */
 
/**
*****************************************************************************************************
* 包含头文件
*****************************************************************************************************
*/

#include "BSP_MPU6050.h"
#include "BSP_Delay.h"


/***************************************************************************************************/

/**
*****************************************************************************************************
* 宏定义
*****************************************************************************************************
*/


/***************************************************************************************************/

/**
*****************************************************************************************************
* 全局/静态变量
*****************************************************************************************************
*/



/***************************************************************************************************/

/**
*****************************************************************************************************
* 全局函数
*****************************************************************************************************
*/


uint8_t MPU6050_Init(void)
{ 
    uint8_t res;
    MPU6050_Write_Byte(MPU6050_PWR_MGMT1_REG,0X80); //复位MPU6050
    Delay_ms(100);
    MPU6050_Write_Byte(MPU6050_PWR_MGMT1_REG,0X00); //唤醒MPU6050，因为默认上电是睡眠状态
    MPU6050_Set_Gyro_Fsr(3);                        //陀螺仪传感器,±2000dps
    MPU6050_Set_Accel_Fsr(0);                       //加速度传感器,±2g
    MPU6050_Set_Rate(50);                           //设置采样率50Hz
    MPU6050_Write_Byte(MPU6050_INT_EN_REG,0X00);    //关闭所有中断
    MPU6050_Write_Byte(MPU6050_USER_CTRL_REG,0X00); //I2C主模式关闭
    MPU6050_Write_Byte(MPU6050_FIFO_EN_REG,0X00);   //关闭FIFO
    MPU6050_Write_Byte(MPU6050_INTBP_CFG_REG,0X80); //INT引脚低电平有效
    res = MPU6050_Read_Byte(MPU6050_DEVICE_ID_REG);
    if(res == (MPU6050_ADDR >>1))//器件ID正确
    {
        MPU6050_Write_Byte(MPU6050_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
        MPU6050_Write_Byte(MPU6050_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
        MPU6050_Set_Rate(50);                           //设置采样率为50Hz
    }
    else 
    {
        return 1;
    }
    return 0;
}
//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU6050_Set_Gyro_Fsr(uint8_t fsr)
{
    return MPU6050_Write_Byte(MPU6050_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围  
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU6050_Set_Accel_Fsr(uint8_t fsr)
{
    return MPU6050_Write_Byte(MPU6050_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围  
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU6050_Set_LPF(uint16_t lpf)
{
    uint8_t data = 0;
    if(lpf >= 188)data = 1;
    else if(lpf >= 98) data = 2;
    else if(lpf >= 42) data = 3;
    else if(lpf >= 20)   data = 4;
    else if(lpf >= 10)   data = 5;
    else data = 6; 
    return MPU6050_Write_Byte(MPU6050_CFG_REG,data);//设置数字低通滤波器  
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU6050_Set_Rate(uint16_t rate)
{
    uint8_t data;
    if(rate > 1000) rate = 1000;
    if(rate < 4)rate = 4;
    data = 1000/rate - 1;
    data = MPU6050_Write_Byte(MPU6050_SAMPLE_RATE_REG,data);	//设置数字低通滤波器
    return MPU6050_Set_LPF(rate/2);	//自动设置LPF为采样率的一半
}

//得到温度值
//返回值:温度值(扩大了100倍)
short MPU6050_Get_Temperature(void)
{
    uint8_t buf[2]; 
    short raw;
    float temp;
    MPU6050_Read_Len(MPU6050_ADDR,MPU6050_TEMP_OUTH_REG,2,buf); 
    raw = ((uint16_t)buf[0]<<8)|buf[1];  
    temp = 36.53+((double)raw)/340;  
    return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU6050_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    uint8_t buf[6],res;  
    res = MPU6050_Read_Len(MPU6050_ADDR,MPU6050_GYRO_XOUTH_REG,6,buf);
    if(res == 0)
    {
        *gx = ((uint16_t)buf[0]<<8) | buf[1];  
        *gy = ((uint16_t)buf[2]<<8) | buf[3];  
        *gz = ((uint16_t)buf[4]<<8) | buf[5];
    }
    return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU6050_Get_Accelerometer(short *ax,short *ay,short *az)
{
    uint8_t buf[6],res;  
    res=MPU6050_Read_Len(MPU6050_ADDR,MPU6050_ACCEL_XOUTH_REG,6,buf);
    if(res == 0)
    {
        *ax = ((uint16_t)buf[0] <<8) | buf[1];  
        *ay = ((uint16_t)buf[2] <<8) | buf[3];  
        *az = ((uint16_t)buf[4] <<8) | buf[5];
    }
    return res;;
}
//I2C连续写
//addr:器件地址 
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
//    其他,错误代码
uint8_t MPU6050_Write_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
    return MPU6050_Mem_Write(MPU6050_ADDR,reg,1,buf,len);
} 
//I2C连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
//    其他,错误代码
uint8_t MPU6050_Read_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
    return MPU6050_Mem_Read(MPU6050_ADDR,reg,1,buf,len);
}



//陀螺仪方向设置
static signed char gyro_orientation[9] = { 1, 0, 0,
                                           0, 1, 0,
                                           0, 0, 1};
//MPU6050自测试
//返回值:0,正常
//    其他,失败
uint8_t run_self_test(void)
{
    int result;
    //char test_packet[4] = {0};
    long gyro[3], accel[3]; 
    result = mpu_run_self_test(gyro, accel);
    if (result == 0x3) 
    {
        float sens;
        unsigned short accel_sens;
        mpu_get_gyro_sens(&sens);
        gyro[0] = (long)(gyro[0] * sens);
        gyro[1] = (long)(gyro[1] * sens);
        gyro[2] = (long)(gyro[2] * sens);
        dmp_set_gyro_bias(gyro);
        mpu_get_accel_sens(&accel_sens);
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        dmp_set_accel_bias(accel);
        return 0;
    }else return 1;
}
//陀螺仪方向控制
unsigned short inv_orientation_matrix_to_scalar(
    const signed char *mtx)
{
    unsigned short scalar; 
    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;


    return scalar;
}
//方向转换
unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;      // error
    return b;
}

//mpu6050,dmp初始化
//返回值:0,正常
//    其他,失败
uint8_t mpu_dmp_init(void)
{
    uint8_t res=0;
    if(mpu_init()==0)	//初始化MPU6050
    {
        res = mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
        if(res)return 1; 
        res = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
        if(res)return 2; 
        res = mpu_set_sample_rate(DEFAULT_MPU_HZ);  //设置采样率
        if(res)return 3; 
        res = dmp_load_motion_driver_firmware();    //加载dmp固件
        if(res)return 4; 
        res = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
        if(res)return 5; 
        res = dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|	//设置dmp功能
        DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
        DMP_FEATURE_GYRO_CAL);
        if(res)return 6; 
        res = dmp_set_fifo_rate(DEFAULT_MPU_HZ);    //设置DMP输出速率(最大不超过200Hz)
        if(res)return 7;   
        res = run_self_test();  //自检
        if(res)return 8;    
        res = mpu_set_dmp_state(1);	//使能DMP
        if(res)return 9;     
    }
    return 0;
}
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°
//roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°
//yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°
//返回值:0,正常
//    其他,失败
uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
    float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
    unsigned long sensor_timestamp;
    short gyro[3], accel[3], sensors;
    unsigned char more;
    long quat[4]; 
    if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;    
    if(sensors&INV_WXYZ_QUAT) 
    {
        q0 = quat[0] / q30;	//q30格式转换为浮点数
        q1 = quat[1] / q30;
        q2 = quat[2] / q30;
        q3 = quat[3] / q30; 
        //计算得到俯仰角/横滚角/航向角
        *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch
        *roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
        *yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw
    }else return 2;
    return 0;
}
/***************************************************************************************************/

/**
*****************************************************************************************************
* static函数
*****************************************************************************************************
*/

//I2C写一个字节 
//reg:寄存器地址
//data:数据
//返回值:0,正常
//    其他,错误代码
static uint8_t MPU6050_Write_Byte(uint8_t reg,uint8_t data)
{
    return MPU6050_Mem_Write(MPU6050_ADDR,reg,1,&data,1);
}
//I2C读一个字节 
//reg:寄存器地址 
//返回值:读到的数据
static uint8_t MPU6050_Read_Byte(uint8_t reg)
{
    uint8_t res;
    MPU6050_Mem_Read(MPU6050_ADDR,reg,1,&res,1);
    return res;
}

/***************************************************************************************************/

/****************************************************************************************************
  End Of File
****************************************************************************************************/



